function x = rsipopt( x0, funcs, options )
%RSIPOPT Summary of this function goes here
%   Detailed explanation goes here
ncon = length(options.cl); nvar = length(options.lb);
idxceq = find(options.cl == options.cu);
idxcineq = find(options.cl ~= options.cu);
idxbeq = find(options.lb == options.ub);
idxbineq = find(options.lb ~= options.ub ...
    & ~(isinf(options.lb) & isinf(options.ub)) );
neq = length(idxceq) + length(idxbeq); 
nineq = length(idxcineq) + length(idxbineq); 
g_l = [options.cl(idxcineq); options.lb(idxbineq)];
g_u = [options.cu(idxcineq); options.ub(idxbineq)];
g_l(g_l == -Inf) = -2e19; g_u(g_u == Inf) = 2e19;
ceq = options.cl(idxceq); beq = options.lb(idxbeq);
% initial primal-dual
if isempty(options.lambda), lambda = ones(ncon, 1); end
x = x0; l = (g_u-g_l)/2; u = (g_u-g_l)/2;
y = -ones(neq,1); z = 10./l; w = -10./u; %????
tole = options.ipopt.tol/(2*nineq);
% case 9: gamma = 0.97; ^3 0.11
% case 39: gamma = 0.94; ^3 0.11
% case 118: gamma = 0.993; ^3 0.16
gamma = 0.993;
% reduce-space permute
%Ppermu = [(nvar-neq+1):nvar ,  1:(nvar-neq)]';
%Ppermu = randperm(nvar)';%
Ppermu = (1:nvar)';
P = sparse(1:nvar, Ppermu,1, nvar,nvar);
Yr = [speye(neq); sparse(nvar-neq,neq)];
% iteration
for iter = 1 : options.ipopt.max_iter % maxiter
    lambda(idxceq) = -y(1:length(idxceq));
    lambda(idxcineq) = -( z(1:length(idxcineq)) + w(1:length(idxcineq)) );
    % feval
    c = feval(funcs.constraints, x);
    Jf = feval(funcs.gradient, x);
    Jc = feval(funcs.jacobian, x);
    H = feval(funcs.hessian, x, 1, lambda'); % sigma = 1
    % form A b
    Jh = [Jc(idxceq, :); sparse(1:length(idxbeq), ...
        idxbeq, 1, length(idxbeq), nvar)]';
    Jg = [Jc(idxcineq, :); sparse(1:length(idxbineq), ...
        idxbineq, 1, length(idxbineq), nvar)]';
    gap = l'*z - u'*w; fprintf('gap=%f\n',gap);
    
    if gap < tole, break; end              % converge
    %
    Lx = Jf - Jh*y - Jg*(z+w); 
    Ly = [c(idxceq) - ceq; x(idxbeq) - beq];        % h(x)
    gx = [c(idxcineq); x(idxbineq)];
    Lz = gx - l - g_l; Lw = gx + u - g_u;
    % [predictor]: set mu = 0; Llmu = l.*z - mu; Lumu = u.*w + mu;
    %Lx1 = Lx + Jg *((Llmu + z.*Lz)./l + (Lumu - w.*Lw)./u);
    Lx1 = Lx + Jg *(z + z.*Lz./l + w - w.*Lw./u);
    % solve Ax=b
    H1 = -(H + H') + diag(diag(H));
    H1 = H1 -Jg *diag(z./l-w./u)*Jg';
% Code using full space
%   A = sparse([H1,Jh ; Jh',zeros(neq,neq)]); b = [Lx1; -Ly];
%   dxy = klu(A,'\',b); 
%   dx = dxy(1:nvar); dy = dxy(nvar+1 : end);
    %% reduced-space [first predictor]
    %A = sparse([H1,Jh ; Jh',zeros(neq,neq)]); b = [Lx1; -Ly];
  	%dxy = klu(A,'\',b); 
  	%dxp = dxy(1:nvar); dyp = dxy(nvar+1 : end);
    Jh = (P*Jh)'; H1 = P*H1*P'; Lxr = P*Lx1;
    Ck = Jh(:,1:neq); Nk = Jh(:,neq+1:nvar);
    try CkLU = klu(Ck); catch err,
        rk = sprank(Ck); sz = size(Ck);
        disp([rk sz]);
        error('Space decompose error o(Q_Q)o'); 
    end
    Z = full([-klu(CkLU,'\',full(Nk));speye(nvar-neq)]);
    py = -klu(CkLU,'\',full(Ly)); rHes=full(Z'*H1*Z);
    dxy = Yr*py; rw=full(Z'*H1*dxy); opz=full(Z'*Lxr)-rw;
    pz = rHes\opz; dxp = dxy + Z*pz; dxp = P'*dxp;
    % finish predictor step
    Jgdx = Jg'*dxp; dlp = Lz+Jgdx; dup = -Lw-Jgdx; 
    dzp = -z -z.*dlp./l; dwp= -w -w.*dup./u;        
    il = find(dlp<0); iu = find(dup<0); iz = find(dzp<0); iw = find(dwp>0);
    alpha_p = gamma* min(1 , min([-l(il)./dlp(il);-u(iu)./dup(iu)]) );
    alpha_d = gamma* min(1 , min([-z(iz)./dzp(iz);-w(iw)./dwp(iw)]) );
    lp = l + alpha_p*dlp; up = u + alpha_p*dup;
    zp = z + alpha_d*dzp; wp = w + alpha_d*dwp;
    gap_af = lp'*zp - up'*wp; 
    mu = min((gap_af/gap)^3, 0.16) * gap/(2*nineq);
    % [second corrector] ozlp = -mu./l; owup = mu./u;
    ozlp = (dzp.*dlp-mu)./l; owup = (dwp.*dup+mu)./u;
    Lx1 = Lx1 + Jg *(ozlp + owup); 
    
    %b = [Lx1; -Ly];
  	%dxy = klu(A,'\',b); 
  	%dx = dxy(1:nvar); dy = dxy(nvar+1 : end);
    Lxr = P*Lx1; opz = full(Z'*Lxr)-rw;
    pz = rHes\opz; dx = dxy + Z*pz; cdy = Lxr-H1*dx;
    dyT = klu(cdy(1:neq)','/', CkLU); 
    dy = dyT'; dx = P'*dx;
    %% end reduced-space
    % finish damp
    Jgdx = Jg'*dx; dl = Lz + Jgdx; du = -Lw - Jgdx;
    % [Corrector:]
    dz = -z -ozlp -z.*dl./l; dw = -w -owup -w.*du./u;
    il = find(dl<0); iu = find(du<0); iz = find(dz<0); iw = find(dw>0);
    alpha_p = gamma* min(1 , min([-l(il)./dl(il);-u(iu)./du(iu)]) );
    alpha_d = gamma* min(1 , min([-z(iz)./dz(iz);-w(iw)./dw(iw)]) );
    x = x + alpha_p * dx; l = l + alpha_p * dl; u = u + alpha_p * du; 
    y = y + alpha_d * dy;  z = z + alpha_d * dz; w = w + alpha_d * dw; 
end
% final
fprintf('converge after %d times, solution is:\n', iter);
end

